Intoduction¶

https://gist.github.com/radcli14/1e5923cc3927debc755aada7f40eb22c

https://moorepants.github.io/learn-multibody-dynamics/configuration.html#calculating-additional-kinematic-quantities

In [784]:
import math  # provides pi as a float
import numpy as np
import sympy as sp
import sympy.physics.mechanics as me
import pandas as pd

# sp.init_printing(use_latex='mathjax')
# me.init_vprinting()
me.mechanics_printing()
In [785]:
from IPython.display import display, Latex

def dtex(*args):
    tex = ''
    for arg in args:
        if isinstance(arg, str):
            # Handle string arguments
            tex += fr'${arg}$ $\, = \,$ '
        else:
            # Handle SymPy arguments
            tex += fr'${me.mlatex(arg)}$ $\, = \,$ '
    
    # Trim off the last equals sign
    tex = tex[:-10]
    display(Latex(tex))

Define Variables¶

In [786]:
"""
Non-time-varying symbols

These will need be to be replaced with a constant, numeric value in the simulation step.
"""
l_a, l_c, l_n = sp.symbols('l_a l_c l_n')

"""
Time-varying symbols
"""
q = theta_1, theta_2, l_b = me.dynamicsymbols(r'theta_1 theta_2 l_b') # Generalized angular coordinates
s = omega_1, omega_2, ldot_b = me.dynamicsymbols(r'omega_1 omega_2 \dot{l_b}') # Generalized angular speeds
theta_3, theta_o = me.dynamicsymbols(r'theta_3 theta_o') # Generalized coordinates
omega_3, omega_o = me.dynamicsymbols(r'omega_3 omega_o') # Generalized speeds
theta_i = me.dynamicsymbols(r'theta_i')

t = me.dynamicsymbols._t
q, s
Out[786]:
$\displaystyle \left( \left[ \theta_{1}, \ \theta_{2}, \ l_{b}\right], \ \left[ \omega_{1}, \ \omega_{2}, \ \dot{l_b}\right]\right)$
In [787]:
"""
Create derivative of each generalized coordinates (diff(q)) $qd$ to correspond to 
each generalized speed $s$.

The kinematic equations are created as the variable named kd with 
each derivative of a generalized coordinate being subtracted by the 
corresponding generalized speed. 
Convention for the kinematic equations is that each item in the list is assumed 
to be equal to zero, thus the sk - diff(qk) can be assume to be analogous to 
sk == diff(qk) from the perspective of the equation of motion generation step to follow.
"""
kd = [sk - sp.diff(qk) for qk, sk in zip(q, s)]
fk = sp.Matrix(kd)
qd = sp.Matrix(q).diff(t)
Mk = fk.jacobian(qd)
gk = fk.xreplace({s_i: 0 for s_i in qd})
s_sol = -Mk.LUsolve(gk)
s_repl = dict(zip(qd, s_sol))

# OR just
# s_repl = {
#     theta_1.diff(t): omega_1,
#     theta_2.diff(t): omega_2,
#     l_b.diff(t): dotl_b,
# }

s_repl
Out[787]:
$\displaystyle \left\{ \dot{l}_{b} : \dot{l_b}, \ \dot{\theta}_{1} : \omega_{1}, \ \dot{\theta}_{2} : \omega_{2}\right\}$

Kinematics¶

Frames¶

In [788]:
# Inertial Reference Frame
N = me.ReferenceFrame('N')

A = me.ReferenceFrame('A')
B = me.ReferenceFrame('B')
C = me.ReferenceFrame('C')

A.orient_axis(N, N.z, theta_1)
B.orient_axis(N, N.z, theta_2)

theta_3 = theta_2 - sp.pi/2
C.orient_axis(N, N.z, theta_3) 
# C.orient_axis(B, B.z, me.dot(B.x, C.x)) # TODO: this might work as it constrains the rotation of C with respect to B

OF = me.ReferenceFrame('OF')

theta_o = l_b/l_c + theta_3 - sp.pi
OF.orient_axis(N, N.z, theta_o)

A.set_ang_vel(N, omega_1*N.z)
B.set_ang_vel(N, omega_2*N.z)
omega_3 = omega_2
C.set_ang_vel(N, omega_3*N.z)

omega_o = ldot_b/l_c + omega_3
OF.set_ang_vel(N, omega_o*N.z)

print(f'Rotation matrix for the A frame')
dtex('\\mathbf{R}_A', A.dcm(N))
print(f'Angular rate for the A frame')
dtex('\\mathbf{\\omega}_A', A.ang_vel_in(N))

print(f'Rotation matrix for the B frame')
dtex('\\mathbf{R}_B', B.dcm(N))
print(f'Angular rate for the B frame')
dtex('\\mathbf{\\omega}_B', B.ang_vel_in(N))

print(f'Rotation matrix for the C frame')
dtex('\\mathbf{R}_C', sp.trigsimp(C.dcm(N)))
print(f'Angular rate for the C frame')
dtex('\\mathbf{\\omega}_C', C.ang_vel_in(N))


print(f'Rotation matrix for the OF frame')
dtex('\\mathbf{R}_{OF}', sp.trigsimp(OF.dcm(N)))
print(f'Angular rate for the C frame')
dtex('\\mathbf{\\omega}_{OF}', OF.ang_vel_in(N))

Rotation matrix for the A frame
$\mathbf{R}_A$ $\, = \,$ $\left[\begin{matrix}\cos{\left(\theta_{1} \right)} & \sin{\left(\theta_{1} \right)} & 0\\- \sin{\left(\theta_{1} \right)} & \cos{\left(\theta_{1} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$ 
Angular rate for the A frame
$\mathbf{\omega}_A$ $\, = \,$ $\omega_{1}\mathbf{\hat{n}_z}$ 
Rotation matrix for the B frame
$\mathbf{R}_B$ $\, = \,$ $\left[\begin{matrix}\cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} & 0\\- \sin{\left(\theta_{2} \right)} & \cos{\left(\theta_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$ 
Angular rate for the B frame
$\mathbf{\omega}_B$ $\, = \,$ $\omega_{2}\mathbf{\hat{n}_z}$ 
Rotation matrix for the C frame
$\mathbf{R}_C$ $\, = \,$ $\left[\begin{matrix}\sin{\left(\theta_{2} \right)} & - \cos{\left(\theta_{2} \right)} & 0\\\cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$ 
Angular rate for the C frame
$\mathbf{\omega}_C$ $\, = \,$ $\omega_{2}\mathbf{\hat{n}_z}$ 
Rotation matrix for the OF frame
$\mathbf{R}_{OF}$ $\, = \,$ $\left[\begin{matrix}- \sin{\left(\theta_{2} + \frac{l_{b}}{l_{c}} \right)} & \cos{\left(\theta_{2} + \frac{l_{b}}{l_{c}} \right)} & 0\\- \cos{\left(\theta_{2} + \frac{l_{b}}{l_{c}} \right)} & - \sin{\left(\theta_{2} + \frac{l_{b}}{l_{c}} \right)} & 0\\0 & 0 & 1\end{matrix}\right]$ 
Angular rate for the C frame
$\mathbf{\omega}_{OF}$ $\, = \,$ $(\omega_{2} + \frac{\dot{l_b}}{l_{c}})\mathbf{\hat{n}_z}$
In [789]:
"""
Check that frame C is 90 degrees rotated from frame B
"""
me.dot(B.x, C.x)
Out[789]:
$\displaystyle 0$

Points¶

In [790]:
# Origin: Stationary point
O = me.Point('O')
O.set_vel(N, 0)

P1 = me.Point('P1')
P2 = me.Point('P2')
P3 = me.Point('P3')
P4 = me.Point('P4')

# Output point
PO = me.Point('PO')

P1.set_pos(O, 0)
P2.set_pos(P1, l_a*A.x)
P3.set_pos(P2, l_b*B.x)
P4.set_pos(P3, l_c*C.x)

PO.set_pos(P4, l_c*OF.x)

# P4.pos_from(P1)
# P2.pos_from(P1)
PO.pos_from(P4)
Out[790]:
$\displaystyle l_{c}\mathbf{\hat{of}_x}$

Constraints¶

In [791]:
# Constraint equations
# loop
fc = P4.pos_from(P1) - l_n*N.x
fv = fc.dt(N)

# Constraint matrix equations in inertial frame
position_constraint = sp.Matrix(fc.to_matrix(N)[:2])
# OR
# fx = sp.trigsimp(loop.dot(N.x))
# fy = sp.trigsimp(loop.dot(N.y))
# position_constraint = sp.Matrix([fx, fy])

# velocity_constraint = sp.Matrix(fv.to_matrix(N)[:2])
# OR
velocity_constraint = position_constraint.diff(t).subs(s_repl)

print('Position constraint equation')
dtex(fc, '0')

print('Position constraint equations in inertial frame')
dtex(position_constraint)
display(me.find_dynamicsymbols(position_constraint))

print('Velocity constraint equation')
dtex(fv, '0')

print('Velocity constraint equations in inertial frame')
dtex(velocity_constraint)
display(me.find_dynamicsymbols(velocity_constraint))

Position constraint equation
$l_{c}\mathbf{\hat{c}_x} + l_{b}\mathbf{\hat{b}_x} + l_{a}\mathbf{\hat{a}_x} - l_{n}\mathbf{\hat{n}_x}$ $\, = \,$ $0$ 
Position constraint equations in inertial frame
$\left[\begin{matrix}l_{a} \cos{\left(\theta_{1} \right)} + l_{c} \sin{\left(\theta_{2} \right)} - l_{n} + l_{b} \cos{\left(\theta_{2} \right)}\\l_{a} \sin{\left(\theta_{1} \right)} - l_{c} \cos{\left(\theta_{2} \right)} + l_{b} \sin{\left(\theta_{2} \right)}\end{matrix}\right]$
$\displaystyle \left\{l_{b}, \theta_{1}, \theta_{2}\right\}$ 
Velocity constraint equation
$l_{c} \omega_{2}\mathbf{\hat{c}_y} + \dot{l}_{b}\mathbf{\hat{b}_x} + l_{b} \omega_{2}\mathbf{\hat{b}_y} + l_{a} \omega_{1}\mathbf{\hat{a}_y}$ $\, = \,$ $0$ 
Velocity constraint equations in inertial frame
$\left[\begin{matrix}- l_{a} \omega_{1} \sin{\left(\theta_{1} \right)} + l_{c} \omega_{2} \cos{\left(\theta_{2} \right)} + \dot{l_b} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)}\\l_{a} \omega_{1} \cos{\left(\theta_{1} \right)} + l_{c} \omega_{2} \sin{\left(\theta_{2} \right)} + \dot{l_b} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$
$\displaystyle \left\{\dot{l_b}, l_{b}, \omega_{1}, \omega_{2}, \theta_{1}, \theta_{2}\right\}$
In [792]:
P1.set_vel(N, 0)
P4.vel(N)
Out[792]:
$\displaystyle l_{c} \omega_{2}\mathbf{\hat{c}_y} + \dot{l}_{b}\mathbf{\hat{b}_x} + l_{b} \omega_{2}\mathbf{\hat{b}_y} + l_{a} \omega_{1}\mathbf{\hat{a}_y}$

Analysis¶

Calculating Additional Kinematic Quantities¶

In [793]:
display(velocity_constraint)
display(me.find_dynamicsymbols(velocity_constraint))
# display(velocity_constraint.xreplace(s_repl))
# display(me.find_dynamicsymbols(velocity_constraint.xreplace(s_repl)))
$\displaystyle \left[\begin{matrix}- l_{a} \omega_{1} \sin{\left(\theta_{1} \right)} + l_{c} \omega_{2} \cos{\left(\theta_{2} \right)} + \dot{l_b} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)}\\l_{a} \omega_{1} \cos{\left(\theta_{1} \right)} + l_{c} \omega_{2} \sin{\left(\theta_{2} \right)} + \dot{l_b} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$
$\displaystyle \left\{\dot{l_b}, l_{b}, \omega_{1}, \omega_{2}, \theta_{1}, \theta_{2}\right\}$

We can see that the expressions are linear in $\theta_1$, $\theta_2$ and $\theta_3$. If we select $\theta_2$ and $\theta_3$ to be dependent, we can solve the linear system $Ax=b$

In [794]:
# x = sp.Matrix([theta_2.diff(t), l_b.diff(t)])
x = sp.Matrix([omega_2, ldot_b])
x
Out[794]:
$\displaystyle \left[\begin{matrix}\omega_{2}\\\dot{l_b}\end{matrix}\right]$
In [795]:
velocity_constraint
Out[795]:
$\displaystyle \left[\begin{matrix}- l_{a} \omega_{1} \sin{\left(\theta_{1} \right)} + l_{c} \omega_{2} \cos{\left(\theta_{2} \right)} + \dot{l_b} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)}\\l_{a} \omega_{1} \cos{\left(\theta_{1} \right)} + l_{c} \omega_{2} \sin{\left(\theta_{2} \right)} + \dot{l_b} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$
In [796]:
position_constraint.diff(t).xreplace(s_repl)
Out[796]:
$\displaystyle \left[\begin{matrix}- l_{a} \omega_{1} \sin{\left(\theta_{1} \right)} + l_{c} \omega_{2} \cos{\left(\theta_{2} \right)} + \dot{l_b} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)}\\l_{a} \omega_{1} \cos{\left(\theta_{1} \right)} + l_{c} \omega_{2} \sin{\left(\theta_{2} \right)} + \dot{l_b} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)}\end{matrix}\right]$
In [797]:
Amatrix = velocity_constraint.jacobian(x)
# EQUIVALENT TO
# Amatrix = position_constraint.diff(t).xreplace(s_repl).jacobian(x)
Amatrix
Out[797]:
$\displaystyle \left[\begin{matrix}l_{c} \cos{\left(\theta_{2} \right)} - l_{b} \sin{\left(\theta_{2} \right)} & \cos{\left(\theta_{2} \right)}\\l_{c} \sin{\left(\theta_{2} \right)} + l_{b} \cos{\left(\theta_{2} \right)} & \sin{\left(\theta_{2} \right)}\end{matrix}\right]$
In [798]:
# b = -velocity_constraint.xreplace({theta_2.diff(t): 0, l_b.diff(t): 0})
b = -position_constraint.diff(t).xreplace(s_repl).xreplace({omega_2: 0, ldot_b: 0})
b
Out[798]:
$\displaystyle \left[\begin{matrix}l_{a} \omega_{1} \sin{\left(\theta_{1} \right)}\\- l_{a} \omega_{1} \cos{\left(\theta_{1} \right)}\end{matrix}\right]$

solve for the dependent variables

In [799]:
x_sol = sp.simplify(-Amatrix.LUsolve(b))
x_sol
Out[799]:
$\displaystyle \left[\begin{matrix}\frac{l_{a} \omega_{1} \cos{\left(\theta_{1} - \theta_{2} \right)}}{l_{b}}\\- \frac{l_{a} \left(l_{c} \cos{\left(\theta_{1} - \theta_{2} \right)} + l_{b} \sin{\left(\theta_{1} - \theta_{2} \right)}\right) \omega_{1}}{l_{b}}\end{matrix}\right]$

Now we can write any velocity strictly in terms of the independent speed $\theta_1$ and all of the other coordinates. free_dynamicsymbols() shows us what coordinates and their time derivatives present an any vector:

In [800]:
qd_dep_repl = {
  theta_2.diff(t): x_sol[0],
  l_b.diff(t): x_sol[1],
}
qd_dep_repl

# s_dep_repl = dict(zip(x, x_sol))
# OR
s_dep_repl = {
  omega_2: x_sol[0],
  ldot_b: x_sol[1],
}
s_dep_repl
Out[800]:
$\displaystyle \left\{ \dot{l_b} : - \frac{l_{a} \left(l_{c} \cos{\left(\theta_{1} - \theta_{2} \right)} + l_{b} \sin{\left(\theta_{1} - \theta_{2} \right)}\right) \omega_{1}}{l_{b}}, \ \omega_{2} : \frac{l_{a} \omega_{1} \cos{\left(\theta_{1} - \theta_{2} \right)}}{l_{b}}\right\}$

Now we have unknown dependant variables of velocity_constraint as equations of known variables

In [801]:
P4.vel(N).to_matrix(N)
Out[801]:
$\displaystyle \left[\begin{matrix}- l_{a} \omega_{1} \sin{\left(\theta_{1} \right)} + l_{c} \omega_{2} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)} + \cos{\left(\theta_{2} \right)} \dot{l}_{b}\\l_{a} \omega_{1} \cos{\left(\theta_{1} \right)} + l_{c} \omega_{2} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)} + \sin{\left(\theta_{2} \right)} \dot{l}_{b}\\0\end{matrix}\right]$
In [802]:
me.find_dynamicsymbols(P4.vel(N).to_matrix(N))
Out[802]:
$\displaystyle \left\{l_{b}, \omega_{1}, \omega_{2}, \theta_{1}, \theta_{2}, \dot{l}_{b}\right\}$
In [803]:
display(P4.vel(N).xreplace(qd_dep_repl))
display(me.find_dynamicsymbols(P4.vel(N).xreplace(qd_dep_repl).to_matrix(N)))
$\displaystyle l_{c} \omega_{2}\mathbf{\hat{c}_y} - \frac{l_{a} \left(l_{c} \cos{\left(\theta_{1} - \theta_{2} \right)} + l_{b} \sin{\left(\theta_{1} - \theta_{2} \right)}\right) \omega_{1}}{l_{b}}\mathbf{\hat{b}_x} + l_{b} \omega_{2}\mathbf{\hat{b}_y} + l_{a} \omega_{1}\mathbf{\hat{a}_y}$
$\displaystyle \left\{l_{b}, \omega_{1}, \omega_{2}, \theta_{1}, \theta_{2}\right\}$
In [804]:
display(P4.vel(N).xreplace(s_repl).xreplace(s_dep_repl))
display(me.find_dynamicsymbols(P4.vel(N).xreplace(s_repl).xreplace(s_dep_repl).to_matrix(N)))
$\displaystyle \frac{l_{a} l_{c} \omega_{1} \cos{\left(\theta_{1} - \theta_{2} \right)}}{l_{b}}\mathbf{\hat{c}_y} - \frac{l_{a} \left(l_{c} \cos{\left(\theta_{1} - \theta_{2} \right)} + l_{b} \sin{\left(\theta_{1} - \theta_{2} \right)}\right) \omega_{1}}{l_{b}}\mathbf{\hat{b}_x} + l_{a} \omega_{1} \cos{\left(\theta_{1} - \theta_{2} \right)}\mathbf{\hat{b}_y} + l_{a} \omega_{1}\mathbf{\hat{a}_y}$
$\displaystyle \left\{l_{b}, \omega_{1}, \theta_{1}, \theta_{2}\right\}$
In [805]:
# fhdt = position_constraint.diff(t)
v_P4 = sp.Matrix(P4.vel(N).xreplace(s_repl).xreplace(s_dep_repl).to_matrix(N)[:2])
v_P4 = sp.trigsimp(v_P4) # warning, this simplification is slow and may not be necessary
v_P4
Out[805]:
$\displaystyle \left[\begin{matrix}- 2 l_{a} \omega_{1} \sin{\left(\theta_{1} \right)}\\2 l_{a} \omega_{1} \cos{\left(\theta_{1} \right)}\end{matrix}\right]$

Solve¶

Setup¶

In [823]:
num_sprockets = 4
angular_offset = np.pi/2 # Offset between input lever and second elliptical gear; to be optimized
num_steps = 30

# Create a dictionary of constants
repl = {
    l_a: 1.0,
    # l_b: unknown,
    l_c: 1.0,
    l_n: 4.0,
    theta_1: 0, # for demonstration purposes, will be replaced with a function of time
    # theta_2: unknown,
    # theta_3: unknown,
    # theta_o: unknown,

    # omega_1: # unknown
    # omega_2: # unknown
    # omega_3: # unknown
    # omega_o: # unknown
}
display(repl)

e = sp.symbols('e')
input_subs = {
    theta_i.diff(t): 1,
    e: 0.7
}
$\displaystyle \left\{ l_{a} : 1.0, \ l_{c} : 1.0, \ l_{n} : 4.0, \ \theta_{1} : 0\right\}$

Solve the elliptical gear input¶

In [824]:
r_i, r_1 = sp.symbols('r_i, r_1')

meshing_elliptical_gears_eq = sp.Eq(r_i*theta_i.diff(t), r_1*theta_1.diff(t))
display(meshing_elliptical_gears_eq)

ellipse_subs = {
    r_i: 1/sp.sqrt(1 - (e*sp.cos(theta_i))**2),
    r_1: 1/sp.sqrt(1 - (e*sp.cos(theta_1))**2),
}

dtex(r"r_i", ellipse_subs[r_i])
dtex(r"r_1", ellipse_subs[r_1])

display(meshing_elliptical_gears_eq.xreplace(ellipse_subs))
ellipse_eq_for_thetadot_1 = sp.simplify(sp.solve(meshing_elliptical_gears_eq.xreplace(ellipse_subs), theta_1.diff(t))[0])

dtex(theta_1.diff(t), ellipse_eq_for_thetadot_1)

thetadot_1 = sp.Eq(theta_1.diff(t), ellipse_eq_for_thetadot_1).xreplace(input_subs)
display(thetadot_1)

print('we are going to use theta_1 as y and theta_i as t')
print()

import scipy
f = sp.lambdify((theta_i, theta_1), thetadot_1.rhs, cse=False)

def solve_elliptical_gear(start=0, stop=2*np.pi, num_steps=9, start_offset=np.pi/2):
    args = []
    y0 = [start_offset]
    # theta_i_eval = np.linspace(0, 2*np.pi, num_steps)
    theta_i_eval = np.linspace(start, stop, num_steps)
    solution = scipy.integrate.solve_ivp(f, (start, stop), y0, t_eval=theta_i_eval, args=args, method='RK45')
    # print(solution)

    theta_i_sol = solution.t # which is the same as theta_i_eval
    theta_1_sol = np.array(solution.y[0], dtype=np.float64)
    return theta_i_sol, theta_1_sol

# Test the function
off = np.pi/2
start = 0 + off
stop = 2*np.pi + off
# theta_i_sol = np.linspace(start, stop, num_steps)
theta_i_sol, theta_1_sol = solve_elliptical_gear(start, stop, num_steps=num_steps, start_offset=off+np.pi/2)

# Plot test results
import plotly.graph_objects as go
fig = go.Figure()
fig.add_trace(go.Scatter(x=theta_i_sol, y=theta_1_sol, mode='lines+markers', name='theta_i vs theta_1'))
fig.update_layout(title='theta_i vs theta_1 test', xaxis_title='theta_i', yaxis_title='theta_1')
fig.update_layout(height=800)
fig.show()
$\displaystyle r_{i} \dot{\theta}_{i} = r_{1} \dot{\theta}_{1}$
$r_i$ $\, = \,$ $\frac{1}{\sqrt{- e^{2} \cos^{2}{\left(\theta_{i} \right)} + 1}}$
$r_1$ $\, = \,$ $\frac{1}{\sqrt{- e^{2} \cos^{2}{\left(\theta_{1} \right)} + 1}}$
$\displaystyle \frac{\dot{\theta}_{i}}{\sqrt{- e^{2} \cos^{2}{\left(\theta_{i} \right)} + 1}} = \frac{\dot{\theta}_{1}}{\sqrt{- e^{2} \cos^{2}{\left(\theta_{1} \right)} + 1}}$
$\dot{\theta}_{1}$ $\, = \,$ $\frac{\sqrt{- e^{2} \cos^{2}{\left(\theta_{1} \right)} + 1} \dot{\theta}_{i}}{\sqrt{- e^{2} \cos^{2}{\left(\theta_{i} \right)} + 1}}$
$\displaystyle \dot{\theta}_{1} = \frac{\sqrt{1 - 0.49 \cos^{2}{\left(\theta_{1} \right)}}}{\sqrt{1 - 0.49 \cos^{2}{\left(\theta_{i} \right)}}}$ 
we are going to use theta_1 as y and theta_i as t
In [825]:
display(position_constraint.xreplace(repl))
display(me.find_dynamicsymbols(position_constraint.xreplace(repl)))
$\displaystyle \left[\begin{matrix}l_{b} \cos{\left(\theta_{2} \right)} + 1.0 \sin{\left(\theta_{2} \right)} - 3.0\\l_{b} \sin{\left(\theta_{2} \right)} - 1.0 \cos{\left(\theta_{2} \right)}\end{matrix}\right]$
$\displaystyle \left\{l_{b}, \theta_{2}\right\}$
In [826]:
display(velocity_constraint.xreplace(repl))
display(me.find_dynamicsymbols(velocity_constraint.xreplace(repl)))
$\displaystyle \left[\begin{matrix}\dot{l_b} \cos{\left(\theta_{2} \right)} - l_{b} \omega_{2} \sin{\left(\theta_{2} \right)} + 1.0 \omega_{2} \cos{\left(\theta_{2} \right)}\\\dot{l_b} \sin{\left(\theta_{2} \right)} + l_{b} \omega_{2} \cos{\left(\theta_{2} \right)} + 1.0 \omega_{1} + 1.0 \omega_{2} \sin{\left(\theta_{2} \right)}\end{matrix}\right]$
$\displaystyle \left\{\dot{l_b}, l_{b}, \omega_{1}, \omega_{2}, \theta_{2}\right\}$
In [827]:
theta_2_guess = 35.0*math.pi/180.0
l_b_guess = 4.0

omega_2_guess = 1.0
ldot_b_guess = 1.0

sol = sp.nsolve(position_constraint.xreplace(repl), (theta_2, l_b), (theta_2_guess, l_b_guess))
sol
Out[827]:
$\displaystyle \left[\begin{matrix}0.339836909454122\\2.82842712474619\end{matrix}\right]$
In [851]:
def solve_4barlink(values: dict):
    theta_2_sol, l_b_sol = sp.nsolve(
        position_constraint.xreplace(values), (theta_2, l_b), (theta_2_guess, l_b_guess)
    )
    values[theta_2] = float(theta_2_sol)
    values[l_b] = float(l_b_sol)

    # Option 1: Use linear solver to find remaining dependant variables
    # theta_2_dt_sol, l_b_dt_sol = sp.nsolve(velocity_constraint.subs(repl_diff), (theta_2.diff(t), l_b.diff(t)), (theta_2_diff_t_guess, l_b_diff_t_guess))
    omega_2_sol, ldot_b_sol = sp.nsolve(
        velocity_constraint.xreplace(s_repl).xreplace(values),
        (omega_2, ldot_b),
        (omega_2_guess, ldot_b_guess),
    )
    values[omega_2] = float(omega_2_sol)
    values[ldot_b] = float(ldot_b_sol)

    # Options 2: Use `s_dep_repl` that has omega_2_sol and ldot_b_sol as equations of solved variables to directly solve for the remaining speeds
    # display(OF.ang_vel_in(N).to_matrix(N).xreplace(qd_dep_repl).simplify())
    # display(OF.ang_vel_in(N).to_matrix(N).xreplace(qd_dep_repl).xreplace(repl_dep).simplify())
    # display(OF.ang_vel_in(N).to_matrix(N).xreplace(s_repl).simplify())
    # display(OF.ang_vel_in(N).to_matrix(N).xreplace(s_repl).xreplace(repl_dep).simplify())

    # display(PO.vel(N).to_matrix(N).xreplace(qd_dep_repl).xreplace(repl_dep))

    # Misc calculations
    values[theta_3] = float(theta_3.subs(values))
    values[theta_o] = float(theta_o.subs(values))
    values[omega_o] = float(omega_o.subs(values))
    # angle between C and OF
    theta_c_o = float(theta_3.subs(values) + np.pi - theta_o.subs(values))
    values["arclen"] = float(2 * np.pi * values[l_c] * theta_c_o / (2 * np.pi))
    values["chainlen"] = float(values["arclen"] + values[l_b])
    return values


solutions = pd.DataFrame(
    columns=[
        "sprocket",
        "time_step_idx",
        theta_i,
        theta_1,
        theta_2,
        theta_3,
        theta_o,
        omega_1,
        omega_2,
        omega_o,
        l_b,
        ldot_b,
    ]
)
for i in range(num_sprockets):
    time_step_idx = 0
    sprocket_offset = i * (2 * np.pi / num_sprockets) + angular_offset

    start = 0 + sprocket_offset
    stop = 2*np.pi + sprocket_offset
    theta_i_sol, theta_1_sol = solve_elliptical_gear(start, stop, num_steps, start_offset=sprocket_offset+np.pi/2)
    theta_i_range = np.max(theta_i_sol) - np.min(theta_i_sol)
    theta_1_range = np.max(theta_1_sol) - np.min(theta_1_sol)
    print(f"theta_i range: {theta_i_range}, theta_1 range: {theta_1_range}")
    for theta_i_val, theta_1_val in zip(theta_i_sol, theta_1_sol):
        values = repl.copy()
        values[theta_i] = theta_i_val
        values[theta_1] = theta_1_val
        values[omega_1] = float(ellipse_eq_for_thetadot_1.subs(input_subs).subs(values))
        # adjust theta_1 since the elliptical gear is offset from the lever
        values[theta_1] = theta_1_val - angular_offset
        values = solve_4barlink(values)
        values["sprocket"] = i + 1
        values["time_step_idx"] = time_step_idx
        solutions = pd.concat(
            [solutions, pd.DataFrame(values, index=[0])], ignore_index=True
        )
        time_step_idx += 1

display(solutions)

theta_i range: 6.283185307179586, theta_1 range: 6.3017637563120275

C:\Users\phcre\AppData\Local\Temp\ipykernel_22292\2845617415.py:74: FutureWarning:

The behavior of DataFrame concatenation with empty or all-NA entries is deprecated. In a future version, this will no longer exclude empty or all-NA columns when determining the result dtypes. To retain the old behavior, exclude the relevant entries before the concat operation.


theta_i range: 6.283185307179586, theta_1 range: 6.251882663708635
theta_i range: 6.283185307179586, theta_1 range: 6.016566519568924
theta_i range: 6.283185307179586, theta_1 range: 6.297514855309432
sprocket time_step_idx theta_i(t) theta_1(t) theta_2(t) theta_2(t) - pi/2 theta_2(t) - 3*pi/2 + l_b(t)/l_c omega_1(t) omega_2(t) omega_2(t) + \dot{l_b}(t)/l_c l_b(t) \dot{l_b}(t) l_a l_c l_n arclen chainlen
0 1 0 1.570796 1.570796 -1.373825e-23 -1.570796 -0.712389 0.714143 -1.093223e-17 0.714143 4.000000 0.714143 1.0 1.0 4.0 2.283185 6.283185
1 1 1 1.787458 1.726375 2.907889e-03 -1.567888 -0.557420 0.730653 2.676174e-02 0.722154 4.152061 0.695392 1.0 1.0 4.0 2.131124 6.283185
2 1 2 2.004119 1.887757 1.156805e-02 -1.559228 -0.400420 0.781234 5.462103e-02 0.745086 4.300400 0.690465 1.0 1.0 4.0 1.982785 6.283185
3 1 3 2.220781 2.066439 2.696206e-02 -1.543834 -0.235171 0.869835 8.829001e-02 0.776036 4.450256 0.687746 1.0 1.0 4.0 1.832929 6.283185
4 1 4 2.437443 2.271211 5.092396e-02 -1.519872 -0.061870 0.998737 1.313196e-01 0.795387 4.599595 0.664067 1.0 1.0 4.0 1.683590 6.283185
... ... ... ... ... ... ... ... ... ... ... ... ... ... ... ... ... ...
115 4 25 11.699724 11.554133 5.234206e-01 -1.047376 -0.759642 0.902769 -9.248248e-03 -0.902212 3.429327 -0.892964 1.0 1.0 4.0 2.853858 6.283185
116 4 26 11.916386 11.762328 5.144857e-01 -1.056311 -0.965321 1.040105 -8.031082e-02 -1.007185 3.232583 -0.926874 1.0 1.0 4.0 3.050603 6.283185
117 4 27 12.133047 12.004777 4.852928e-01 -1.085503 -1.189212 1.201562 -1.978697e-01 -1.040396 3.037884 -0.842526 1.0 1.0 4.0 3.245302 6.283185
118 4 28 12.349709 12.281389 4.258793e-01 -1.144917 -1.401607 1.343397 -3.528806e-01 -0.876548 2.884903 -0.523668 1.0 1.0 4.0 3.398282 6.283185
119 4 29 12.566371 12.580700 3.350446e-01 -1.235752 -1.548772 1.400210 -4.697823e-01 -0.441409 2.828572 0.028373 1.0 1.0 4.0 3.454613 6.283185

120 rows × 17 columns

Plot¶

In [840]:
import plotly
plotly.offline.init_notebook_mode()
plotly.io.renderers.default = 'notebook'
import plotly.graph_objects as go
In [841]:
# Plot the mechanism

fig = go.Figure()

solutions_sprocket1 = solutions[solutions['sprocket'] == 1]
# solutions_sprocket1 = solutions[solutions['time_step_idx'] == 1]

# solutions_sprocket1 = solutions_sprocket1[solutions_sprocket1['time_step_idx'] > 20 ]

display(solutions_sprocket1)
for i, sol in solutions_sprocket1.iterrows():
    sol = sol.to_dict()
    fig.add_shape(
        type='line',
        x0=float(P1.pos_from(O).to_matrix(N).subs(sol)[0]),
        y0=float(P1.pos_from(O).to_matrix(N).subs(sol)[1]),
        x1=float(P2.pos_from(O).to_matrix(N).subs(sol)[0]),
        y1=float(P2.pos_from(O).to_matrix(N).subs(sol)[1]),
        line=dict(color='blue', width=2),
    )
    fig.add_shape(
        type='line',
        x0=float(P2.pos_from(O).to_matrix(N).subs(sol)[0]),
        y0=float(P2.pos_from(O).to_matrix(N).subs(sol)[1]),
        x1=float(P3.pos_from(O).to_matrix(N).subs(sol)[0]),
        y1=float(P3.pos_from(O).to_matrix(N).subs(sol)[1]),
        line=dict(color='red', width=2),
    )
    fig.add_shape(
        type='line',
        x0=float(P3.pos_from(O).to_matrix(N).subs(sol)[0]),
        y0=float(P3.pos_from(O).to_matrix(N).subs(sol)[1]),
        x1=float(P4.pos_from(O).to_matrix(N).subs(sol)[0]),
        y1=float(P4.pos_from(O).to_matrix(N).subs(sol)[1]),
        line=dict(color='green', width=2),
    )
    fig.add_shape(
        type='line',
        x0=float(P4.pos_from(O).to_matrix(N).subs(sol)[0]),
        y0=float(P4.pos_from(O).to_matrix(N).subs(sol)[1]),
        x1=float(PO.pos_from(O).to_matrix(N).subs(sol)[0]),
        y1=float(PO.pos_from(O).to_matrix(N).subs(sol)[1]),
        line=dict(color='orange', width=2),
    )

fig.update_layout(
    # height=800,
    yaxis_scaleanchor="x",
)
fig.show()
sprocket time_step_idx theta_i(t) theta_1(t) theta_2(t) theta_2(t) - pi/2 theta_2(t) - 3*pi/2 + l_b(t)/l_c omega_1(t) omega_2(t) omega_2(t) + \dot{l_b}(t)/l_c l_b(t) \dot{l_b}(t) l_a l_c l_n arclen chainlen
0 1 0 0.000000 3.141593 0.201358 -1.369438 0.387948 1.400280 0.280056 0.280056 4.898979 1.432874e-16 1.0 1.0 4.0 1.384206 6.283185
1 1 1 0.216662 3.440173 0.262107 -1.308689 0.412438 1.340829 0.275553 -0.048894 4.862719 -3.244468e-01 1.0 1.0 4.0 1.420466 6.283185
2 1 2 0.433323 3.715551 0.318525 -1.252271 0.372483 1.197722 0.243134 -0.302622 4.766347 -5.457558e-01 1.0 1.0 4.0 1.516838 6.283185
3 1 3 0.649985 3.958587 0.366934 -1.203863 0.288693 1.035677 0.201233 -0.450540 4.634148 -6.517733e-01 1.0 1.0 4.0 1.649037 6.283185
4 1 4 0.866646 4.168157 0.406388 -1.164408 0.181994 0.898392 0.162899 -0.522126 4.487994 -6.850249e-01 1.0 1.0 4.0 1.795191 6.283185
5 1 5 1.083308 4.350494 0.438061 -1.132735 0.065301 0.800159 0.132264 -0.557501 4.339629 -6.897647e-01 1.0 1.0 4.0 1.943556 6.283185
6 1 6 1.299969 4.516682 0.464014 -1.106783 -0.058426 0.740093 0.108259 -0.584795 4.189950 -6.930541e-01 1.0 1.0 4.0 2.093236 6.283185
7 1 7 1.516631 4.673841 0.485301 -1.085495 -0.188733 0.715166 0.088585 -0.619262 4.038354 -7.078474e-01 1.0 1.0 4.0 2.244831 6.283185
8 1 8 1.733292 4.829316 0.502529 -1.068268 -0.328274 0.723451 0.070101 -0.670330 3.881586 -7.404303e-01 1.0 1.0 4.0 2.401599 6.283185
9 1 9 1.949954 4.990543 0.515523 -1.055273 -0.481580 0.765704 0.048463 -0.744234 3.715286 -7.926962e-01 1.0 1.0 4.0 2.567899 6.283185
10 1 10 2.166616 5.164977 0.522886 -1.047910 -0.653737 0.844903 0.016784 -0.842816 3.535766 -8.596008e-01 1.0 1.0 4.0 2.747419 6.283185
11 1 11 2.383277 5.360097 0.521297 -1.049499 -0.848471 0.963363 -0.036336 -0.955676 3.342621 -9.193404e-01 1.0 1.0 4.0 2.940564 6.283185
12 1 12 2.599939 5.583403 0.504533 -1.066264 -1.064587 1.115550 -0.127173 -1.041470 3.143269 -9.142976e-01 1.0 1.0 4.0 3.139916 6.283185
13 1 13 2.816600 5.842700 0.462646 -1.108150 -1.289398 1.275447 -0.266759 -1.001570 2.960345 -7.348111e-01 1.0 1.0 4.0 3.322840 6.283185
14 1 14 3.033262 6.136672 0.386618 -1.184179 -1.482233 1.385192 -0.419532 -0.703999 2.843538 -2.844672e-01 1.0 1.0 4.0 3.439647 6.283185
15 1 15 3.249923 6.440502 0.285951 -1.284845 -1.580601 1.384083 -0.482335 -0.177549 2.845837 3.047860e-01 1.0 1.0 4.0 3.437348 6.283185
16 1 16 3.466585 6.732519 0.185994 -1.384803 -1.560909 1.273086 -0.414501 0.331394 2.965486 7.458949e-01 1.0 1.0 4.0 3.317699 6.283185
17 1 17 3.683247 6.998022 0.107546 -1.463250 -1.449150 1.110437 -0.288966 0.633665 3.155693 9.226308e-01 1.0 1.0 4.0 3.127492 6.283185
18 1 18 3.899908 7.229279 0.055699 -1.515098 -1.291998 0.955794 -0.178707 0.742959 3.364692 9.216658e-01 1.0 1.0 4.0 2.918493 6.283185
19 1 19 4.116570 7.425528 0.025301 -1.545496 -1.126711 0.838496 -0.103233 0.753647 3.560377 8.568796e-01 1.0 1.0 4.0 2.722809 6.283185
20 1 20 4.333231 7.592975 0.009062 -1.561734 -0.970289 0.762679 -0.054508 0.735033 3.733038 7.895416e-01 1.0 1.0 4.0 2.550147 6.283185
21 1 21 4.549893 7.744795 0.001531 -1.569266 -0.821354 0.722855 -0.020534 0.718429 3.889504 7.389638e-01 1.0 1.0 4.0 2.393681 6.283185
22 1 22 4.766554 7.897442 0.000234 -1.570563 -0.668942 0.715304 0.007645 0.714636 4.043213 7.069907e-01 1.0 1.0 4.0 2.239972 6.283185
23 1 23 4.983216 8.051453 0.004634 -1.566162 -0.516154 0.740325 0.033848 0.726603 4.191601 6.927546e-01 1.0 1.0 4.0 2.091584 6.283185
24 1 24 5.199877 8.217475 0.015026 -1.555770 -0.356357 0.800523 0.062968 0.752409 4.341006 6.894411e-01 1.0 1.0 4.0 1.942179 6.283185
25 1 25 5.416539 8.405205 0.032849 -1.537947 -0.186229 0.900523 0.099299 0.782217 4.493311 6.829185e-01 1.0 1.0 4.0 1.789875 6.283185
26 1 26 5.633201 8.620517 0.059898 -1.510898 -0.010385 1.040031 0.145466 0.790995 4.642106 6.455288e-01 1.0 1.0 4.0 1.641079 6.283185
27 1 27 5.849862 8.865460 0.097504 -1.473293 0.158053 1.202262 0.199482 0.734104 4.772938 5.346223e-01 1.0 1.0 4.0 1.510247 6.283185
28 1 28 6.066524 9.138264 0.145778 -1.425019 0.298970 1.343113 0.250650 0.562701 4.865581 3.120508e-01 1.0 1.0 4.0 1.417604 6.283185
29 1 29 6.283185 9.433333 0.203070 -1.367726 0.389631 1.400255 0.280437 0.270656 4.898950 -9.781308e-03 1.0 1.0 4.0 1.384236 6.283185
In [842]:
# plot l_b, arclen, and chainlen over theta_1
fig = go.Figure()
fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_1], y=solutions_sprocket1[l_b], mode='lines+markers', name=f"${sp.latex(l_b)}$"))
fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_1], y=solutions_sprocket1['arclen'], mode='lines+markers', name='arclen'))
fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_1], y=solutions_sprocket1['chainlen'], mode='lines+markers', name='chainlen'))
fig.update_layout(title='l_b, arclen, and chainlen over theta_1', xaxis_title=f"${sp.latex(theta_1)}$", yaxis_title=f"length")
In [843]:
# plot theta_i vs theta_1
fig = go.Figure()
fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_i], y=solutions_sprocket1[theta_1], mode='lines+markers', name=f"${sp.latex(theta_i)}$ vs ${sp.latex(theta_1)}$"))
fig.update_layout(title='theta_i vs theta_1', xaxis_title=f"${sp.latex(theta_i)}$", yaxis_title=f"${sp.latex(theta_1)}$")
fig.show()
# get range of theta_i and theta_1
theta_i_range = solutions_sprocket1[theta_i].max() - solutions_sprocket1[theta_i].min()
theta_1_range = solutions_sprocket1[theta_1].max() - solutions_sprocket1[theta_1].min()
theta_i_range, theta_1_range
Out[843]:
$\displaystyle \left( 6.28318530717959, \ 6.29174066338906\right)$
In [844]:
fig = go.Figure()
for i in range(num_sprockets):
    solutions_sprocket = solutions[solutions['sprocket'] == i+1]
    fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_i], y=solutions_sprocket[omega_1], mode='lines+markers', name=f"${i+1}{sp.latex(theta_i)}$"))
fig.update_layout(title='theta_i vs omega_1', xaxis_title=f"${sp.latex(theta_i)}$", yaxis_title=f"${sp.latex(omega_1)}" + r"\text{ or } \dot{\theta_i}$")
In [845]:
# plot theta_i vs theta_o
fig = go.Figure()

for i in range(num_sprockets):
    solutions_sprocket = solutions[solutions['sprocket'] == i+1]
    fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_i], y=solutions_sprocket[theta_o], mode='lines+markers', name='theta_1'))
fig.update_layout(title='Input Angle vs Output Angle', xaxis_title=f'Input Angle (${sp.latex(theta_i)}$)', yaxis_title=r'Output Angle (${\theta_o}(t)$)')
In [846]:
# plot omega_o vs theta_1
fig = go.Figure()

for i in range(num_sprockets):
    solutions_sprocket = solutions[solutions['sprocket'] == i+1]
    # set negative values to 0
    omega_o_sprocket = np.where(solutions_sprocket[omega_o] < 0, 0, solutions_sprocket[omega_o])
    # fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_1], y=omega_o_sprocket, mode='lines+markers', name=f'omega_o sprocket {i+1}'))
    # fig.add_trace(go.Scatter(x=solutions_sprocket1[theta_i], y=omega_o_sprocket, mode='lines+markers', name=f'omega_o sprocket {i+1}'))
    fig.add_trace(go.Scatter(x=solutions_sprocket1['time_step_idx'], y=omega_o_sprocket, mode='lines+markers', name=f'omega_o sprocket {i+1}'))

fig.update_layout(title='Input Angle vs Output Speed', xaxis_title=f'Input Angle (${sp.latex(theta_i)}$)', yaxis_title=r'Output Speed ($\omega_o(t)$)')
# fig.update_xaxes(range=[0, 2*np.pi])
In [847]:
# plot omega_o_max vs theta_1
fig = go.Figure()

# for each theta_1, get max omega_o
solutions_max_omega = pd.DataFrame(columns=["time_step_idx", theta_i, "max_omega_o"])
for step in range(num_steps):
    # print(theta)
    max_at_theta_i = 0
    solutions_step = solutions[solutions["time_step_idx"] == step]
    max_omega_o = solutions_step[omega_o].max()
    solutions_max_omega = pd.concat(
        [
            solutions_max_omega,
            pd.DataFrame(
                {
                    "time_step_idx": [step],
                    theta_i: [solutions_step.iloc[0][theta_i]],
                    "max_omega_o": [max_omega_o],
                }
            ),
        ],
        ignore_index=True,
    )

# display(solutions_max_omega)

# log range of max-min of omega_o_max
print("max omega_o_max", solutions_max_omega["max_omega_o"].max())
print("min omega_o_max", solutions_max_omega["max_omega_o"].min())
print("range omega_o_max", solutions_max_omega["max_omega_o"].max() - solutions_max_omega["max_omega_o"].min())

fig.add_trace(
    go.Scatter(
        x=solutions_max_omega[theta_i],
        y=solutions_max_omega["max_omega_o"],
        mode="lines+markers",
        name="max omega_o",
    )
)
fig.update_layout(
    title="Input Angle vs Max Output Speed",
    xaxis_title=f"Input Angle (${sp.latex(theta_i)}$)",
    yaxis_title=r"Max Output Speed ($\omega_o(t)$)",
)
# set y scale to 0-1
fig.update_yaxes(range=[0, 1.5])

max omega_o_max 0.8031721878693989
min omega_o_max 0.7007048364199682
range omega_o_max 0.10246735144943075

C:\Users\phcre\AppData\Local\Temp\ipykernel_22292\532369578.py:11: FutureWarning:

The behavior of DataFrame concatenation with empty or all-NA entries is deprecated. In a future version, this will no longer exclude empty or all-NA columns when determining the result dtypes. To retain the old behavior, exclude the relevant entries before the concat operation.

Animate¶

In [848]:
import manim as mm
In [852]:
%%manim -qm -v WARNING DiscreteSteps3

solutions_sprocket_anim = solutions[solutions['sprocket'] == 1]

def vec_sp2mm(eq):
    """convert sympy vector to numpy array ready for manim"""
    return np.array(eq).astype(np.float64).reshape(1, 3)[0]


class DiscreteSteps3(mm.MovingCameraScene):
    def point_point(self, Point, subs, color=mm.WHITE):
        loc = vec_sp2mm(Point.pos_from(O).to_matrix(N).subs(subs))
        # point = mm.Point(location=loc, color=color)
        point = mm.Dot(point=loc, color=color)
        return point

    def line_2points(self, P1, P2, subs, color=mm.WHITE):
        start = vec_sp2mm(P1.pos_from(O).to_matrix(N).subs(subs))
        end = vec_sp2mm(P2.pos_from(O).to_matrix(N).subs(subs))
        line = mm.Line(start=start, end=end, color=color)
        return line

    def line_label(self, name, line, color=mm.WHITE, percent=0.5):
        # get vector midpoint
        # midpoint = (line.get_start() + line.get_end()) / 2
        # get normalized vector of line
        vector = line.get_end() - line.get_start()
        # get vector of length 0.5
        midpoint = line.get_start() + percent * vector

        # get vector perpendicular to the line and normalize it
        normal = np.array(
            [
                -(line.get_end()[1] - line.get_start()[1]),
                line.get_end()[0] - line.get_start()[0],
                0,
            ]
        )
        # normal /= np.linalg.norm(normal)
        # above can cause a zero division error
        if np.linalg.norm(normal) != 0:
            normal /= np.linalg.norm(normal)
        label = mm.Text(name, font_size=24).next_to(midpoint, normal, buff=0.1)
        return label

    def point_label(self, name, P, subs, direction=mm.UP, color=mm.WHITE):
        point = vec_sp2mm(P.pos_from(O).to_matrix(N).subs(subs))
        label = mm.Text(name, font_size=12).next_to(point, direction, buff=0.1)
        return label

    def arc_2points_dashed(self, P1, P2, Pc, subs, color=mm.WHITE):
        start = vec_sp2mm(P1.pos_from(O).to_matrix(N).subs(subs))
        end = vec_sp2mm(P2.pos_from(O).to_matrix(N).subs(subs))
        center = vec_sp2mm(Pc.pos_from(O).to_matrix(N).subs(subs))
        radius = np.linalg.norm(center - start)
        start_angle = np.arctan2(start[1] - center[1], start[0] - center[0])
        end_angle = np.arctan2(end[1] - center[1], end[0] - center[0])
        angle = end_angle - start_angle
        length = radius * angle
        num_dashes = int(np.abs(length) * 10)
        arc = mm.Arc(radius, start_angle, angle=angle, arc_center=center, color=color)
        return arc

    def add_ref_frame(self, line, name, color=mm.WHITE, buff=0.1, scale=0.5):
        midpoint = (line.get_start() + line.get_end()) / 2
        unit_vector = line.get_unit_vector() * scale
        # normal needs to be reversed
        # normal_unit_vector = np.array([unit_vector[1], -unit_vector[0], 0])
        normal_unit_vector = np.array([-unit_vector[1], unit_vector[0], 0])
        spacing = buff * normal_unit_vector
        # print(midpoint)
        # print(unit_vector)
        # print(normal_unit_vector)
        # print(spacing)
        x_axis = mm.Arrow(start=midpoint + spacing, end=midpoint + unit_vector + spacing, color=color, buff=0)
        y_axis = mm.Arrow(start=midpoint + spacing, end=midpoint + normal_unit_vector + spacing, color=color, buff=0)
        # x_str = r"\vec{"+name+r"}_x"
        # y_str = r"\vec{"+name+r"}_y"
        # x_str = name + r"x̂"
        # y_str = name + r"ŷ"
        x_str = r"<i>" + name + r"<sub>x</sub></i>"
        y_str = r"<i>" + name + r"<sub>y</sub></i>"
        x_vec_label = mm.MarkupText(x_str, font_size=12).next_to(midpoint + unit_vector + spacing, unit_vector, buff=0.1)
        y_vec_label = mm.MarkupText(y_str, font_size=12).next_to(midpoint + normal_unit_vector + spacing, normal_unit_vector, buff=0.1)
        group = mm.VGroup(x_axis, y_axis, x_vec_label, y_vec_label)
        return group

    def add_rotation_label(self, line, name, color=mm.WHITE, buff=0.3, scale=0.5):
        start = line.get_start()
        end = line.get_end()
        unit_vector = line.get_unit_vector() * scale
        end_h = start + np.array([1, 0, 0])

        # draw dashed horizontal line
        x_axis = mm.DashedVMobject(mm.Line(start=start, end=end_h, color=mm.GRAY))
        # draw rotation arc with arrow
        vector_h = end_h - start 
        unit_vector_h = vector_h / np.linalg.norm(vector_h) * scale
        end_point = start + unit_vector
        start_point = start + unit_vector_h
        # arc = mm.CurvedArrow(start_point=start+unit_vector_h, end_point=start+unit_vector, tip_length=scale/6, color=color)
        arc = mm.ArcBetweenPoints(start_point, end_point, radius=scale)
        arc.add_tip(tip_length = scale/2*mm.DEFAULT_ARROW_TIP_LENGTH, tip_width=scale/2*mm.DEFAULT_ARROW_TIP_LENGTH)
        # text
        angle = np.arccos(np.dot(unit_vector_h, unit_vector))
        half_angle = angle / 2
        midpoint_arc = np.array([start[0] + np.cos(half_angle) * unit_vector[0], start[1] + np.sin(half_angle) * unit_vector[1], 0])
        deg_label = mm.MarkupText(r"θ<sub>"+name+r"</sub>", font_size=12).next_to(midpoint_arc, mm.RIGHT, buff=buff)
        group = mm.VGroup(x_axis, arc, deg_label)
        return group

    def construct(self):
        scene_center = np.array([2, 1, 0])
        self.play(self.camera.frame.animate.move_to(scene_center))
        # 1. Draw the mechanism
        sol = solutions_sprocket_anim.iloc[0].to_dict()
        # display(sol)

        A_line = self.line_2points(P1, P2, sol, mm.BLUE)
        A_label = self.line_label("A", A_line, mm.BLUE)
        A_outline = mm.DashedVMobject(
            mm.Circle(radius=sol[l_a], color=mm.BLUE).move_to(A_line.get_start())
        )
        B_line = self.line_2points(P2, P3, sol, mm.RED)
        B_label = self.line_label("B", B_line, mm.RED)
        # B_label = self.line_label_leader("B", B_line, mm.RED)
        C_line_ref = self.line_2points(P3, P4, sol, mm.GREEN)
        C_line = mm.DashedVMobject(C_line_ref.copy())
        C_label = self.line_label("C", C_line_ref, mm.GREEN)
        # C_outline = mm.DashedVMobject(mm.Circle(radius=sol[l_c], color=mm.GREEN).move_to(C_line_ref.get_start()))
        N_line_ref = self.line_2points(P4, P1, sol, mm.GRAY)
        N_line = mm.DashedVMobject(N_line_ref.copy())
        N_label = self.line_label("N", N_line_ref, mm.GRAY)
        Output_line = self.line_2points(PO, P4, sol, mm.ORANGE)
        Output_label = self.line_label("Output", Output_line, mm.ORANGE)
        Output_outline = mm.DashedVMobject(
            mm.Circle(radius=sol[l_c], color=mm.ORANGE).move_to(Output_line.get_end())
        )
        Chain_arc = self.arc_2points_dashed(P3, PO, P4, sol, mm.RED)
        # Chain_label = mm.Text("Chain", font_size=24).next_to(Chain_arc, mm.UP, buff=0.1)

        P1_point = self.point_point(P1, sol)
        P1_label = self.point_label("P1", P1, sol, mm.LEFT)
        P2_point = self.point_point(P2, sol)
        P2_label = self.point_label("P2", P2, sol, mm.UP)
        P3_point = self.point_point(P3, sol)
        P3_label = self.point_label("P3", P3, sol, mm.UP)
        P4_point = self.point_point(P4, sol)
        P4_label = self.point_label("P4", P4, sol, mm.RIGHT)
        PO_point = self.point_point(PO, sol)
        PO_label = self.point_label("PO", PO, sol, mm.RIGHT)

        # self.add(P2_point, PO_point)
        self.play(
            mm.Create(A_outline),
            mm.Create(Output_outline),
            mm.Create(A_line),
            mm.Create(B_line),
            mm.Create(Output_line),
            mm.Create(Chain_arc),
            # mm.Create(P1_point),
            mm.Create(P2_point),
            # mm.Create(P3_point),
            # mm.Create(P4_point),
            mm.Create(PO_point),
        )

        if True: # skip for now
            # iterator over solutions
            for i, sol in solutions_sprocket_anim.iterrows():
                sol = sol.to_dict()
                A_line_new = self.line_2points(P1, P2, sol, mm.BLUE)
                B_line_new = self.line_2points(P2, P3, sol, mm.RED)
                # C_line_new = mm.DashedVMobject(self.line_2points(P3, P4, sol, mm.GREEN))
                Output_line_new = self.line_2points(PO, P4, sol, mm.ORANGE)
                Chain_arc_new = self.arc_2points_dashed(P3, PO, P4, sol, mm.RED)
                P2_point_new = self.point_point(P2, sol)
                PO_point_new = self.point_point(PO, sol)

                rate_func = mm.linear
                run_time = 0.5
                self.play(
                    mm.ReplacementTransform(
                        A_line, A_line_new, rate_func=rate_func, run_time=run_time
                    ),
                    mm.ReplacementTransform(
                        B_line, B_line_new, rate_func=rate_func, run_time=run_time
                    ),
                    # mm.Transform(
                    #     C_line, C_line_new, rate_func=rate_func, run_time=run_time
                    # ),
                    mm.ReplacementTransform(
                        Output_line, Output_line_new, rate_func=rate_func, run_time=run_time
                    ),
                    mm.ReplacementTransform(
                        Chain_arc, Chain_arc_new, rate_func=rate_func, run_time=run_time
                    ),
                    mm.ReplacementTransform(
                        P2_point, P2_point_new, rate_func=rate_func, run_time=run_time
                    ),
                    mm.ReplacementTransform(
                        PO_point, PO_point_new, rate_func=rate_func, run_time=run_time
                    ),
                )
                A_line = A_line_new
                B_line = B_line_new
                # C_line = C_line_new
                Output_line = Output_line_new
                Chain_arc = Chain_arc_new
                P2_point = P2_point_new
                PO_point = PO_point_new

        self.wait(1)

        # 2. Show the 4-bar linkage
        self.play(
            mm.Create(N_line),
            mm.Create(C_line),
        )
        self.wait(1)

        # 3. Clean up scene and add labels
        # because we are uncreating and recreating the points, we need to backup the P2_point
        P2_point_bkup = P2_point.copy()
        self.play(
            mm.Uncreate(Output_outline),
            mm.Uncreate(A_outline),
            mm.Uncreate(Output_line),
            mm.Uncreate(Chain_arc),
            mm.Uncreate(P2_point),
            mm.Uncreate(PO_point),
        )
        P2_point = P2_point_bkup
        self.play(
            mm.Write(N_label),
            mm.Write(A_label),
            mm.Write(B_label),
            mm.Write(C_label),
            # mm.Write(Output_label),
        )
        self.play(
            mm.Create(P1_point),
            mm.Create(P2_point),
            mm.Create(P3_point),
            mm.Create(P4_point),
            # mm.Create(PO_point),
            mm.Create(P1_label),
            mm.Create(P2_label),
            mm.Create(P3_label),
            mm.Create(P4_label),
            # mm.Create(PO_label),
        )
        self.wait(1)

        # 4. show the angle references and frames
        A_line.save_state()
        B_line.save_state()
        C_line.save_state()
        P1_point.save_state()
        P2_point.save_state()
        P3_point.save_state()
        P4_point.save_state()
        P1_label.save_state()
        P2_label.save_state()
        P3_label.save_state()
        P4_label.save_state()
        A_label.save_state()
        B_label.save_state()
        C_label.save_state()

        P2_demo = me.Point("P2_demo")
        P3_demo = me.Point("P3_demo")
        P4_demo = me.Point("P4_demo")
        P2_demo.set_pos(O, 1 * N.x + 0.8 * N.y)
        P3_demo.set_pos(O, 2 * N.x + 1.8 * N.y)
        P4_demo.set_pos(O, 3 * N.x + 3.4 * N.y)
        A_line_demo = self.line_2points(P1, P2_demo, sol, mm.BLUE)
        B_line_demo = self.line_2points(P2_demo, P3_demo, sol, mm.RED)
        C_line_ref_demo = self.line_2points(P3_demo, P4_demo, sol, mm.GREEN)
        C_line_demo = mm.DashedVMobject(C_line_ref_demo.copy())
        A_label_demo = self.line_label("A", A_line_demo, percent=0.2)
        B_label_demo = self.line_label("B", B_line_demo, percent=0.2)
        C_label_demo = self.line_label("C", C_line_ref_demo, percent=0.2)
        P1_point_demo = self.point_point(P1, sol)
        P2_point_demo = self.point_point(P2_demo, sol)
        P3_point_demo = self.point_point(P3_demo, sol)
        P4_point_demo = self.point_point(P4_demo, sol)
        P1_label_demo = self.point_label("P1", P1, sol, mm.LEFT)
        P2_label_demo = self.point_label("P2", P2_demo, sol, mm.LEFT)
        P3_label_demo = self.point_label("P3", P3_demo, sol, mm.LEFT)
        P4_label_demo = self.point_label("P4", P4_demo, sol, mm.LEFT)
        self.play(
            mm.Transform(A_line, A_line_demo),
            mm.Transform(B_line, B_line_demo),
            mm.Transform(C_line, C_line_demo),
            mm.Transform(P1_point, P1_point_demo),
            mm.Transform(P2_point, P2_point_demo),
            mm.Transform(P3_point, P3_point_demo),
            mm.Transform(P4_point, P4_point_demo),
            mm.Transform(P1_label, P1_label_demo),
            mm.Transform(P2_label, P2_label_demo),
            mm.Transform(P3_label, P3_label_demo),
            mm.Transform(P4_label, P4_label_demo),
            mm.Transform(A_label, A_label_demo),
            mm.Transform(B_label, B_label_demo),
            mm.Transform(C_label, C_label_demo),
        )
        
        A_frame = self.add_ref_frame(A_line_demo, "a", buff=1.0, color=mm.BLUE)
        B_frame = self.add_ref_frame(B_line_demo, "b", buff=1.0, color=mm.RED)
        C_frame = self.add_ref_frame(C_line_ref_demo, "c", buff=1.0, color=mm.GREEN)
        N_line_ref_reverse = N_line_ref.copy().put_start_and_end_on(N_line_ref.get_end(), N_line_ref.get_start())
        N_frame = self.add_ref_frame(N_line_ref_reverse, "n", buff=-3.0, color=mm.GRAY)
        self.play(
            mm.Create(A_frame),
            mm.Create(B_frame),
            mm.Create(C_frame),
            mm.Create(N_frame),
        )

        A_rot = self.add_rotation_label(A_line_demo, "1", scale=0.5)
        B_rot = self.add_rotation_label(B_line_demo, "2", scale=0.5)
        C_rot = self.add_rotation_label(C_line_ref_demo, "3", scale=0.5)
        self.play(
            mm.Create(A_rot),
            mm.Create(B_rot),
            mm.Create(C_rot),
        )
        self.wait(1)
        self.play(
            mm.Uncreate(A_frame),
            mm.Uncreate(B_frame),
            mm.Uncreate(C_frame),
            mm.Uncreate(N_frame),
            mm.Uncreate(A_rot),
            mm.Uncreate(B_rot),
            mm.Uncreate(C_rot),
        )

        # Move back
        self.play(
            mm.Restore(A_line),
            mm.Restore(B_line),
            mm.Restore(C_line),
            mm.Restore(P1_point),
            mm.Restore(P2_point),
            mm.Restore(P3_point),
            mm.Restore(P4_point),
            mm.Restore(P1_label),
            mm.Restore(P2_label),
            mm.Restore(P3_label),
            mm.Restore(P4_label),
            mm.Restore(A_label),
            mm.Restore(B_label),
            mm.Restore(C_label),
        )



        # Last. Clean up scene
        self.play(
            mm.Unwrite(A_label),
            mm.Unwrite(B_label),
            mm.Unwrite(C_label),
            mm.Unwrite(N_label),
            # mm.Unwrite(Output_label),
        )
        self.play(
            mm.Uncreate(P1_point),
            mm.Uncreate(P2_point),
            mm.Uncreate(P3_point),
            mm.Uncreate(P4_point),
            mm.Unwrite(P1_label),
            mm.Unwrite(P2_label),
            mm.Unwrite(P3_label),
            mm.Unwrite(P4_label),
            # mm.Unwrite(PO_label),
        )
        self.wait(1)

Manim Community v0.19.0
Your browser does not support the video element.

Script¶

  • Consider a mechanism, where an input shaft is a crank, $A$, tied to a connecting rod, $B$. This connecting rod is tied to a chain that is wrapped around another wheel $O$.
  • Look close enough, the core of the mechanism is a classic 4-bar link problem
  • We can define the Reference Frames like so...
    • Instead of having $\theta_2$ and $\theta_3$ as our dependant variables, it will be $\theta_2$ and $l_b$ and $\theta_3$ is just a function of $\theta_2$
  • And then define the Links and Points like so...
  • The output is a simple equation of $\theta_{Output} = f(l_b, \theta_3)$